#include "DSP3D.h"
#include "util/Util.h"
#include "dsp/RingBuffer.h"
using namespace yzrilyzr_util;
using namespace yzrilyzr_lang;
namespace yzrilyzr_dsp{
	void DSP3D::setPos(u_sample yaw, u_sample pitch, u_sample distance){
		this->yaw=Util::clamp(yaw, static_cast<u_sample>(-1.0), static_cast<u_sample>(1.0));
		this->pitch=Util::clamp(pitch, static_cast<u_sample>(-1.0), static_cast<u_sample>(1.0));
		this->distance=std::abs(distance);
		u_sample yaw_rad=this->yaw * Math::PI;
		u_sample pitch_rad=this->pitch * Math::PIDIV2;

		// 3. 球面坐标到笛卡尔坐标的转换
		u_sample cos_pitch=std::cos(pitch_rad);

		this->x=this->distance * std::sin(yaw_rad) * cos_pitch;
		this->y=this->distance * std::sin(pitch_rad);
		this->z=this->distance * std::cos(yaw_rad) * cos_pitch;
		posChange();
	}
	void DSP3D::setPosXYZ(u_sample x, u_sample y, u_sample z){
		// 1. 存储输入的笛卡尔坐标
		this->x=x;
		this->y=y;
		this->z=z;
		// 2. 笛卡尔坐标到球面坐标的转换
		// 计算距离
		this->distance=std::sqrt(x * x + y * y + z * z);

		// 避免除以零的特殊情况 (如果点就在原点)
		if(this->distance < 1e-9){ // 使用一个极小值来判断是否为0
			this->yaw=0;
			this->pitch=0;
			return;
		}

		// 计算俯仰角 (pitch)
		u_sample pitch_rad=std::atan2(y, std::sqrt(x * x + z * z));
		this->pitch=static_cast<u_normal_11>(pitch_rad / Math::PIDIV2);

		// 计算偏航角 (yaw)
		u_sample yaw_rad=std::atan2(x, z);
		this->yaw=static_cast<u_normal_11>(yaw_rad / Math::PI);
		posChange();
	}
	void DSP3D::setChannel(u_index ch){
		this->channel=ch;
	}
	u_index DSP3D::getChannel(){
		return this->channel;
	}
}